﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using BaseDll;
using Communicate;
using SerialDict;
using MotionIoLib;
using System.Threading;
using System.ComponentModel;
using System.Net;
using System.Net.Sockets;
using CommonTools;

namespace MotionIoLib
{
    public class SixRobot_Estun : BaseSixRobot
    {
        string str_IP地址 = "192.168.6.40";//机器人ip  192.168.6.40
        int n_Port = 5000;
        string str_ClientIP = "192.168.6.69";//机器人ip  192.168.6.69
        string str_ServoInput = "";
        public SixRobot_Estun(string strName, params object[] RobotParam) : base(strName, RobotParam)
        {
            this.strName = strName;
            str_IP地址 = RobotParam[0].ToString();
            n_Port = int.Parse(RobotParam[1].ToString());
            str_ClientIP = RobotParam[2].ToString();
            str_ServoInput = RobotParam[3].ToString();
        }
        public Queue<string> m_messageQueue = new Queue<string>();
        private TcpLink link = null;
        bool b_IsConnect = false;
        public override bool Open()
        {
            try
            {
                link = new TcpLink(0, "埃斯顿机器人", "192.168.6.63", 7080, 2000,"");
                link.RecvStringMessageEvent += (object sender, AsyTcpSocketEventArgs e) =>
                {
                    InMessageQueue(e.Message);
                };
                link.SocketErrorEvent += (object sender, AsyTcpSocketEventArgs e) =>
                {
                    return;
                };
                if (link.Open())
                {
                    b_IsConnect = true;
                    return true;
                }
                else
                    return false;
            }
            catch (Exception)
            {
                return false;
            }

        }
        public void clearMessageQueue()
        {
            m_messageQueue.Clear();
        }
        public int OutMessageQueue(out string mes)
        {
            mes = "";
            if (m_messageQueue.Count > 0)
            {
                mes = m_messageQueue.Dequeue();
                return 0;
            }
            else
                return -1;
        }
        public void InMessageQueue(string mes)
        {
            if (mes.Contains("["))
            {
                m_messageQueue.Enqueue(mes);
            }
        }


        public void Send(string msg)
        {
            if (link != null)
            {
                link.WriteString(msg + "\r\n");
            }

        }
        object obj_Lock = new object();
        private int Function_SendCmd(string strCmd, ref string strResult, int n_TimeOut = 2000)
        {
            lock (obj_Lock)
            {
                clearMessageQueue();
                this.Send(strCmd);
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Restart();
                while (true)
                {
                    Thread.Sleep(10);
                    string str_Recevice = "";
                    if (OutMessageQueue(out str_Recevice)==0)
                    {
                        strResult = str_Recevice.Clone().ToString();
                        return 0;
                    }
                    else if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                    {
                        return -4;
                    }
                }


            }
        }
        //设置负载
        public override bool SetPayload(int Scope, string payloadName)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[SetPayload_IFace(" + Scope.ToString() + ",\"" + payloadName + "\"); id=35]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }
        //读取输入信号 type--IO类型14--VAout 13--VAin 12--VDout 11--VDin 2--AAout 1--ADout 0--ADin
        public override bool ReadDigitalInPort(int n_Index)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[GetIOValue(" + n_Index + "," + "0" +");id=10]", ref strMeg);
                //
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return false;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //读取输出信号
        public override bool ReadDigitalOutPort(int n_Index)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd("[GetIOValue(" + n_Index.ToString() + "," + "1" + "); id = 10]", ref strMeg);
                if (n_Result == 0 && strMeg.Contains("[id = 10; Ok; 1]"))
                {
                    return true;
                }
                else if (n_Result == 0 && strMeg.Contains("[id = 10; Ok; 0]"))
                {
                    return false;
                }
                else
                {
                    return false;
                }
                //return false;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }

        }

        //设置输出信号
        public override bool SetDigitalOutputPort(int n_Index, bool b_On)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                string str_Value = b_On ? "1.00" : "0.00";
                n_Result = Function_SendCmd("[SetIOValue(" + n_Index + "," + str_Value + "," + "1" + ");id=9]", ref strMeg); 
                if (n_Result == 0 && strMeg.Contains("Ok"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
                //return false;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }
        public override SixRobotJoint GetCurrentPos_Joint()
        {
            try
            {
                stopwatch.Restart();
                Retry:
                if (stopwatch.ElapsedMilliseconds > 30000)
                    throw new Exception("获取关节坐标系失败");

                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetCurJPos();id=1]", ref strMeg);
                SixRobotJoint PointWorldObject = new SixRobotJoint(0, 0, 0, 0, 0, 0);
                if (strMeg == null)
                    goto Retry;
                if (strMeg.Contains("Ok"))
                {
                    try
                    {
                        string CurrentPos_Joint = strMeg;
                        int StartPosition = CurrentPos_Joint.IndexOf("Ok") + 4;
                        int EndPosition = CurrentPos_Joint.IndexOf("]");
                        string SubCurrentPos_Joint = CurrentPos_Joint.Substring(StartPosition, EndPosition - StartPosition);
                        string[] SubCurrentPos_JointArray = SubCurrentPos_Joint.Split(new char[] { ' ' });
                        PointWorldObject.A1 = double.Parse(SubCurrentPos_JointArray[0]);
                        PointWorldObject.A2 = double.Parse(SubCurrentPos_JointArray[1]);
                        PointWorldObject.A3 = double.Parse(SubCurrentPos_JointArray[2]);
                        PointWorldObject.A4 = double.Parse(SubCurrentPos_JointArray[3]);
                        PointWorldObject.A5 = double.Parse(SubCurrentPos_JointArray[4]);
                        PointWorldObject.A6 = double.Parse(SubCurrentPos_JointArray[5]);
                    }
                    catch (Exception)
                    {
                        goto Retry;
                    }
                }
                else
                {
                    goto Retry;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new SixRobotJoint(0, 0, 0, 0, 0, 0); ;
            }
        }
        Stopwatch stopwatch = new Stopwatch();
        //获取当前位置---世界坐标系
        public override SixRobotPoint GetCurrentPos_World()
        {
            try
            {
                stopwatch.Restart();
                Retry:
                if (stopwatch.ElapsedMilliseconds > 30000)
                    throw new Exception("获取世界坐标系失败");
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[GetCurWPos();id=2]", ref strMeg);
                SixRobotPoint PointWorldObject = new SixRobotPoint(0, 0, 0, 0, 0, 0);
                if (strMeg == null)
                    goto Retry;
                if (strMeg.Contains("Ok"))
                {
                    try
                    {
                        string CurrentPos_Joint = strMeg;
                        int StartPosition = CurrentPos_Joint.IndexOf("Ok") + 4;
                        int EndPosition = CurrentPos_Joint.IndexOf("]");
                        string SubCurrentPos_Joint = CurrentPos_Joint.Substring(StartPosition, EndPosition - StartPosition);
                        string[] SubCurrentPos_JointArray = SubCurrentPos_Joint.Split(new char[] { ' ' });
                        PointWorldObject.X = double.Parse(SubCurrentPos_JointArray[7]);
                        PointWorldObject.Y = double.Parse(SubCurrentPos_JointArray[8]);
                        PointWorldObject.Z = double.Parse(SubCurrentPos_JointArray[9]);
                        PointWorldObject.W = double.Parse(SubCurrentPos_JointArray[10]);
                        PointWorldObject.P = double.Parse(SubCurrentPos_JointArray[11]);
                        PointWorldObject.R = double.Parse(SubCurrentPos_JointArray[12]);
                    }
                    catch (Exception)
                    {
                        goto Retry;
                    }
                }
                else
                {
                    goto Retry;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new SixRobotPoint(0, 0, 0, 0, 0, 0); ;
            }


        }
        //直线运动
        public override bool MoveLine(SixRobotPoint Point, bool bmamual, int Speed = 1000, int BlendValue = 100)
        {
            StationCheck(bmamual);
            string Commond = "[MoveToPos(\"MovL\"," + "\"" + Point.X.ToString() + "_" + Point.Y.ToString() + "_" + Point.Z.ToString() + "_" + Point.W.ToString() + "_" + Point.P.ToString() + "_" + Point.R.ToString() +
                "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + ");id=35]";
            int n_Time = 0;
        Retry:
            int n_Result = 0;
            string strMeg = "";
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, Commond);
            n_Result = Function_SendCmd(Commond, ref strMeg, 6000);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, strMeg);
            if (n_Result != 0)
            {
                n_Time++;
                if(n_Time<3)
                   goto Retry;
            }
            if (n_Result == 0 && !strMeg.Contains("FAIL"))
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    Thread.Sleep(10);
                    SixRobotPoint point = GetCurrentPos_World();
                    b_Result = true;
                    b_Result &= Math.Abs(point.X - Point.X) < 0.5;
                    b_Result &= Math.Abs(point.Y - Point.Y) < 0.5;
                    b_Result &= Math.Abs(point.Z - Point.Z) < 0.5;
                    //b_Result &= Math.Abs(point.P - Point.P) < 0.5;
                    //b_Result &= Math.Abs(point.W - Point.W) < 0.5;
                    //b_Result &= Math.Abs(point.R - Point.R) < 0.5;
                    if (b_Result)
                    {
                        stopwatch.Stop();
                        break;
                    }
                    if (stopwatch.ElapsedMilliseconds > 20000)
                        break;
                }
                if (!b_Result)
                {
                    string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strMeg };
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                    throw (new Exception("机器人运动失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strMeg };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                throw (new Exception("机器人运动失败"));
            }
        }
        //关节运动
        public override bool MoveJog(SixRobotJoint Point, bool bmamual, int Speed = 100, int BlendValue = 100)
        {
            StationCheck(bmamual);
            string Commond = "[MoveToPos(\"MovJ\"," + "\"" + Point.A1.ToString() + "_" + Point.A2.ToString() + "_" + Point.A3.ToString() + "_" + Point.A4.ToString() + "_" + Point.A5.ToString() + "_" + Point.A6.ToString() + 
                "\"," + "\"" + "0" + "_" + Speed.ToString() + "_" + BlendValue.ToString() + "\"" + ");id=35]";
            int n_Time = 0;
        Retry:
            int n_Result = 0;
            string strMeg = "";
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, Commond);
            n_Result = Function_SendCmd(Commond, ref strMeg, 6000);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, strMeg);
            if (n_Result != 0)
            {
                n_Time++;
                if (n_Time < 3)
                    goto Retry;
            }
            if (n_Result == 0 && !strMeg.Contains("FAIL"))
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    Thread.Sleep(10);
                    SixRobotJoint point = GetCurrentPos_Joint();
                    b_Result = true;
                    b_Result &= Math.Abs(point.A1 - Point.A1) < 1;
                    b_Result &= Math.Abs(point.A2 - Point.A2) < 1;
                    b_Result &= Math.Abs(point.A3 - Point.A3) < 1;
                    b_Result &= Math.Abs(point.A4 - Point.A4) < 1;
                    b_Result &= Math.Abs(point.A5 - Point.A5) < 1;
                    b_Result &= Math.Abs(point.A6 - Point.A6) < 1;
                    if (b_Result)
                        break;
                    if (stopwatch.ElapsedMilliseconds > 20000)
                        break;
                }
                if (!b_Result)
                {
                    string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strMeg };
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                    throw (new Exception("机器人运动失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strMeg };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                throw (new Exception("机器人运动失败"));
            }
        }
        private void StationCheck(bool bmanual)
        {
            if (GlobalVariable.g_StationState == StationState.StationStatePause)
            {
                while (true)
                {
                    Thread.Sleep(10);
                    if (GlobalVariable.g_StationState == StationState.StationStateRun)
                        break;
                    else if (GlobalVariable.g_StationState == StationState.StationStatePause)
                        continue;
                    else
                        throw new Exception("自动运行停止");
                }
            }
            if (GlobalVariable.g_StationState == StationState.StationStateRun && bmanual)
            {
                throw new Exception("自动运行的情况下，禁止bmanual=true的运动指令");
            }
            if (GlobalVariable.g_StationState != StationState.StationStateRun && !bmanual)
            {
                throw new Exception("手动运行的情况下，禁止bmanual=false的运动指令");
            }

        }

        public override int SetServo(bool b_Statue)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                int nValue = b_Statue ? 1 : 0;
                n_Result = Function_SendCmd($"[SetMotServoStatus({nValue}); id = 29]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 29; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override bool ReadServoStatue()
        {
            try
            {
                //int n_Result = 0;
                //string strMeg = "";
                //n_Result = Function_SendCmd($"[GetServoSts_IFace(); id = 24]", ref strMeg);
                //if (n_Result == 0 && strMeg == "[id = 24; Ok; 1]")
                //{
                //    return true;
                //}
                //else
                //{
                //    return false;
                //}
                if(str_ServoInput!="")
                   return IOMgr.GetInstace().ReadIoInBit(str_ServoInput);
                return false;

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;

            }

        }
        public override int ResetAlarm()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[ResetError(); id = 15]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 15; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int SetRunSpeed(int Speed)
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"[SetGlobalSpeed({Speed}); id = 24]", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 24; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }
        }
        public override int Stop()
        {
            try
            {
                int n_Result = 0;
                string strMeg = "";
                n_Result = Function_SendCmd($"StopDestPosMotion(); id = 37", ref strMeg);
                if (n_Result == 0 && strMeg == "[id = 37; Ok]")
                {
                    return 0;
                }
                else
                {
                    return -1;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return -1;

            }

        }

        public override bool IsConnect()
        {
            try
            {
                return b_IsConnect;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}"); return false;
            }
        }


    }
}